#include "ros/ros.h"
#include "std_msgs/String.h"
#include "jason_msgs/action.h"

#include <boost/format.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
#include <string>


#include "JasonActionMove.h"
#include "JasonActionTurn.h"

std::list< CJasonAction * >     g_lpoActions;

ros::Subscriber                 g_oActionSub;
ros::NodeHandle *               g_poNode;

CJasonActionTurn::E_Orientation g_eOrientation = CJasonActionTurn::Clockwise;

void actionCallback( const jason_msgs::action::ConstPtr& msg ) {

    const std::string strAction( boost::to_upper_copy( msg->action ) );
    if ( strAction == "MOVE" ) {

        g_lpoActions.push_back( new CJasonActionMove( *g_poNode, msg ) );

    }
    else if ( strAction == "TURN" ) {

        g_lpoActions.push_back( new CJasonActionTurn( *g_poNode, g_eOrientation, msg ) );

    }
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "motion_decomp");
  
  for( int nIndex = 1; nIndex < argc; nIndex++ ) {
      
      if ( !strcmp( argv[ nIndex ], "--clockwise" ) ) {
          
          g_eOrientation = CJasonActionTurn::Clockwise;
      }
      else if ( !strcmp( argv[ nIndex ], "--anticlockwise" ) ) {
      
          g_eOrientation = CJasonActionTurn::Anticlockwise;
      }
  }

  g_poNode = new ros::NodeHandle;
  
  CJasonAction *poCurrAction = NULL;

  g_oActionSub = g_poNode->subscribe( "/jason/action", 100, actionCallback   );
  
  ros::Rate r(10); // 10 hz
  while ( ros::ok() ) {
      
      ros::spinOnce();
      if ( poCurrAction) {
          
          if ( poCurrAction->Done() ) {
          
              delete poCurrAction;
              poCurrAction = NULL;
          }
          else {
              
              poCurrAction->Step();
          }
      }
      else if ( !g_lpoActions.empty() ) {
          
          poCurrAction = g_lpoActions.front();
          g_lpoActions.pop_front();
          poCurrAction->Start();
      }
      r.sleep();
  }
  delete g_poNode;
  
  return 0;
}
